/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University
* of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "include/orbslam/MapPoint.h"
#include "include/orbslam/ORBmatcher.h"

#include <mutex>

namespace SIVO {

long unsigned int MapPoint::nNextId = 0;
std::mutex MapPoint::mGlobalMutex;

MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map *pMap)
    : mnFirstKFid(pRefKF->mnId),
      mnFirstFrame(pRefKF->mnFrameId),
      nObs(0),
      mnTrackReferenceForFrame(0),
      mnLastFrameSeen(0),
      mnBALocalForKF(0),
      mnFuseCandidateForKF(0),
      mnLoopPointForKF(0),
      mnCorrectedByKF(0),
      mnCorrectedReference(0),
      mnBAGlobalForKF(0),
      mpRefKF(pRefKF),
      mnVisible(1),
      mnFound(1),
      mbBad(false),
      mpReplaced(static_cast<MapPoint *>(nullptr)),
      mfMinDistance(0),
      mfMaxDistance(0),
      mpMap(pMap) {
    Pos.copyTo(mWorldPos);
    mNormalVector = cv::Mat::zeros(3, 1, CV_32F);

    // MapPoints can be created from Tracking and Local Mapping. This mutex
    // avoid conflicts with id.
    std::unique_lock<std::mutex> lock(mpMap->mMutexPointCreation);
    mnId = nNextId++;
}

MapPoint::MapPoint(const cv::Mat &Pos,
                   Map *pMap,
                   Frame *pFrame,
                   const int &idxF)
    : mnFirstKFid(-1),
      mnFirstFrame(pFrame->mnId),
      nObs(0),
      mnTrackReferenceForFrame(0),
      mnLastFrameSeen(0),
      mnBALocalForKF(0),
      mnFuseCandidateForKF(0),
      mnLoopPointForKF(0),
      mnCorrectedByKF(0),
      mnCorrectedReference(0),
      mnBAGlobalForKF(0),
      mpRefKF(static_cast<KeyFrame *>(nullptr)),
      mnVisible(1),
      mnFound(1),
      mbBad(false),
      mpReplaced(nullptr),
      mpMap(pMap) {
    Pos.copyTo(mWorldPos);
    cv::Mat Ow = pFrame->GetCameraCenter();
    mNormalVector = mWorldPos - Ow;
    mNormalVector = mNormalVector / cv::norm(mNormalVector);

    cv::Mat PC = Pos - Ow;
    const float dist = static_cast<float>(cv::norm(PC));
    const int level = pFrame->mvKeysSemantic[idxF].octave;
    const float levelScaleFactor = pFrame->mvScaleFactors[level];
    const int nLevels = pFrame->mnScaleLevels;

    mfMaxDistance = dist * levelScaleFactor;
    mfMinDistance = mfMaxDistance / pFrame->mvScaleFactors[nLevels - 1];

    pFrame->mDescriptorsSemantic.row(idxF).copyTo(mDescriptor);

    // MapPoints can be created from Tracking and Local Mapping. This mutex
    // avoid conflicts with id.
    std::unique_lock<std::mutex> lock(mpMap->mMutexPointCreation);
    mnId = nNextId++;
}

void MapPoint::SetSemanticInfo(const Classes &detected_class) {
    if (detected_class != mClass && mClass != Classes::VOID) {
        // If there has been a change in detected class, delete point.
        SetBadFlag();
    } else {
        mClass = detected_class;
    }
}

Classes MapPoint::GetSemanticInfo() {
    return mClass;
}

void MapPoint::SetWorldPos(const cv::Mat &Pos) {
    std::unique_lock<std::mutex> lock(mMutexPos);
    std::unique_lock<std::mutex> lock2(mGlobalMutex);
    Pos.copyTo(mWorldPos);
}

cv::Mat MapPoint::GetWorldPos() {
    std::unique_lock<std::mutex> lock(mMutexPos);
    return mWorldPos.clone();
}

void MapPoint::SetCovariance(const StateCovarianceType &Sigma) {
    std::unique_lock<std::mutex> lock2(mGlobalMutex);
    std::unique_lock<std::mutex> lock(mMutexCovariance);
    mSigma = Sigma;
}

StateCovarianceType MapPoint::GetCovariance() {
    std::unique_lock<std::mutex> lock(mMutexCovariance);
    return mSigma;
}

cv::Mat MapPoint::GetNormal() {
    std::unique_lock<std::mutex> lock(mMutexPos);
    return mNormalVector.clone();
}

KeyFrame *MapPoint::GetReferenceKeyFrame() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return mpRefKF;
}

void MapPoint::AddObservation(KeyFrame *pKF, size_t idx) {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    if (mObservations.count(pKF)) {
        return;
    }

    mObservations[pKF] = idx;

    if (pKF->mvRight[idx] >= 0) {
        nObs += 2;
    } else {
        nObs++;
    }
}

void MapPoint::EraseObservation(KeyFrame *pKF) {
    bool bBad = false;
    {
        std::unique_lock<std::mutex> lock(mMutexFeatures);
        if (mObservations.count(pKF)) {
            int idx = static_cast<int>(mObservations[pKF]);
            if (pKF->mvRight[idx] >= 0)
                nObs -= 2;
            else
                nObs--;

            mObservations.erase(pKF);

            if (mpRefKF == pKF)
                mpRefKF = mObservations.begin()->first;

            // If only 2 observations or less, discard point
            if (nObs <= 2)
                bBad = true;
        }
    }

    if (bBad) {
        SetBadFlag();
    }
}

map<KeyFrame *, size_t> MapPoint::GetObservations() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return mObservations;
}

int MapPoint::Observations() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return nObs;
}

void MapPoint::SetBadFlag() {
    std::map<KeyFrame *, size_t> obs;
    {
        std::unique_lock<std::mutex> lock1(mMutexFeatures);
        std::unique_lock<std::mutex> lock2(mMutexPos);
        mbBad = true;
        obs = mObservations;
        mObservations.clear();
    }

    for (auto mit = obs.begin(); mit != obs.end(); mit++) {
        KeyFrame *pKF = mit->first;
        pKF->EraseMapPointMatch(mit->second);
    }

    mpMap->EraseMapPoint(this);
}

MapPoint *MapPoint::GetReplaced() {
    std::unique_lock<std::mutex> lock1(mMutexFeatures);
    std::unique_lock<std::mutex> lock2(mMutexPos);
    return mpReplaced;
}

void MapPoint::Replace(MapPoint *pMP) {
    if (pMP->mnId == this->mnId) {
        return;
    }

    int nvisible, nfound;
    map<KeyFrame *, size_t> obs;

    {
        std::unique_lock<std::mutex> lock1(mMutexFeatures);
        std::unique_lock<std::mutex> lock2(mMutexPos);
        obs = mObservations;
        mObservations.clear();
        mbBad = true;
        nvisible = mnVisible;
        nfound = mnFound;
        mpReplaced = pMP;
    }

    for (auto mit = obs.begin(), mend = obs.end(); mit != mend; mit++) {
        // Replace measurement in keyframe
        KeyFrame *pKF = mit->first;

        if (!pMP->IsInKeyFrame(pKF)) {
            pKF->ReplaceMapPointMatch(mit->second, pMP);
            pMP->AddObservation(pKF, mit->second);
        } else {
            pKF->EraseMapPointMatch(mit->second);
        }
    }

    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    pMP->ComputeDistinctiveDescriptors();

    mpMap->EraseMapPoint(this);
}

bool MapPoint::isBad() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    std::unique_lock<std::mutex> lock2(mMutexPos);
    return mbBad;
}

void MapPoint::IncreaseVisible(int n) {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    mnVisible += n;
}

void MapPoint::IncreaseFound(int n) {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    mnFound += n;
}

float MapPoint::GetFoundRatio() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return static_cast<float>(mnFound) / mnVisible;
}

void MapPoint::ComputeDistinctiveDescriptors() {
    // Retrieve all observed descriptors
    std::vector<cv::Mat> vDescriptors;

    std::map<KeyFrame *, size_t> observations;

    {
        std::unique_lock<std::mutex> lock1(mMutexFeatures);
        if (mbBad) {
            return;
        }

        observations = mObservations;
    }

    if (observations.empty()) {
        return;
    }

    vDescriptors.reserve(observations.size());

    for (auto mit = observations.begin(); mit != observations.end(); mit++) {
        KeyFrame *pKF = mit->first;

        if (!pKF->isBad())
            vDescriptors.push_back(pKF->mDescriptorsSemantic.row(mit->second));
    }

    if (vDescriptors.empty())
        return;

    // Compute distances between them
    const size_t N = vDescriptors.size();

    float Distances[N][N];
    for (size_t i = 0; i < N; i++) {
        Distances[i][i] = 0;
        for (size_t j = i + 1; j < N; j++) {
            int distij =
              ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);
            Distances[i][j] = distij;
            Distances[j][i] = distij;
        }
    }

    // Take the descriptor with least median distance to the rest
    int BestMedian = INT_MAX;
    int BestIdx = 0;
    for (size_t i = 0; i < N; i++) {
        std::vector<int> vDists(Distances[i], Distances[i] + N);
        std::sort(vDists.begin(), vDists.end());
        int median = vDists[0.5 * (N - 1)];

        if (median < BestMedian) {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        std::unique_lock<std::mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

cv::Mat MapPoint::GetDescriptor() {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return mDescriptor.clone();
}

int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    if (mObservations.count(pKF)) {
        return mObservations[pKF];
    } else {
        return -1;
    }
}

bool MapPoint::IsInKeyFrame(KeyFrame *pKF) {
    std::unique_lock<std::mutex> lock(mMutexFeatures);
    return (mObservations.count(pKF));
}

void MapPoint::UpdateNormalAndDepth() {
    std::map<KeyFrame *, size_t> observations;
    KeyFrame *pRefKF;
    cv::Mat Pos;

    {
        std::unique_lock<std::mutex> lock1(mMutexFeatures);
        std::unique_lock<std::mutex> lock2(mMutexPos);
        if (mbBad) {
            return;
        }

        observations = mObservations;
        pRefKF = mpRefKF;
        Pos = mWorldPos.clone();
    }

    if (observations.empty()) {
        return;
    }

    cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
    int n = 0;
    for (auto mit = observations.begin(); mit != observations.end(); mit++) {
        KeyFrame *pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali / cv::norm(normali);
        n++;
    }

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    const float dist = static_cast<float>(cv::norm(PC));
    const int level = pRefKF->mvKeysSemantic[observations[pRefKF]].octave;
    const float levelScaleFactor = pRefKF->mvScaleFactors[level];
    const int nLevels = pRefKF->mnScaleLevels;

    {
        std::unique_lock<std::mutex> lock3(mMutexPos);
        mfMaxDistance = dist * levelScaleFactor;
        mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
        mNormalVector = normal / n;
    }
}

float MapPoint::GetMinDistanceInvariance() {
    std::unique_lock<std::mutex> lock(mMutexPos);
    return 0.8f * mfMinDistance;
}

float MapPoint::GetMaxDistanceInvariance() {
    std::unique_lock<std::mutex> lock(mMutexPos);
    return 1.2f * mfMaxDistance;
}

int MapPoint::PredictScale(const float &currentDist, KeyFrame *pKF) {
    float ratio;
    {
        std::unique_lock<std::mutex> lock(mMutexPos);
        ratio = mfMaxDistance / currentDist;
    }

    int nScale = static_cast<int>(ceil(log(ratio) / pKF->mfLogScaleFactor));
    if (nScale < 0)
        nScale = 0;
    else if (nScale >= pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels - 1;

    return nScale;
}

int MapPoint::PredictScale(const float &currentDist, Frame *pF) {
    float ratio;
    {
        std::unique_lock<std::mutex> lock(mMutexPos);
        ratio = mfMaxDistance / currentDist;
    }

    int nScale = ceil(log(ratio) / pF->mfLogScaleFactor);
    if (nScale < 0)
        nScale = 0;
    else if (nScale >= pF->mnScaleLevels)
        nScale = pF->mnScaleLevels - 1;

    return nScale;
}


}  // namespace ORB_SLAM
